#include <iostream>
#include "../include/main.h"
#include "../include/api/bulidMap/bulidMapApi.h"

using namespace std;


#define order_none 0
#define order_bulidMay 1
uint32_t returnKey = order_none;
void getKeyboardLoop(void)
{
    char check_keys = ' '; 
    struct termios original_attr, new_attr; // 获取终端的属性
    tcgetattr(STDIN_FILENO, &original_attr);
    new_attr = original_attr;

    // 禁用终端的回显和行缓冲
    new_attr.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &new_attr);

    while (true) {
        char key;
        if (read(STDIN_FILENO, &key, 1) == 1) {
            std::cout<< "get:" << key << endl;
            
            if(key == 'a'){
                returnKey = order_bulidMay;
            }
        }
    }
}

void Callback(const MowerProtoPakg::controlMoveOrder& msg) {
    std::vector<uint8_t> sandSerialBuf(10);
    //printf(" move driver line speed:%f ", msg.linespeed() );
    spdlog::info("move driver line speed:{} angle speed:{}", msg.linespeed() ,msg.angularspeed() );

    // p_chassisDevice->produceSerialData(msg.linespeed(), msg.angularspeed(), sandSerialBuf);
    // p_chassisDevice->sandUsartData(sandSerialBuf);
}

void getNoticLoop(void)
{
    eCAL::protobuf::CSubscriber<MowerProtoPakg::controlMoveOrder> subscriber("ControlMessage");
    subscriber.AddReceiveCallback(std::bind(&Callback, std::placeholders::_2));
    while (eCAL::Ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    eCAL::Finalize();// finalize eCAL API
}

void manageMpc(void)
{
    std::thread thread1 = std::thread(getKeyboardLoop); // 只能这么写，不这么写会报错
    std::thread thread2 = std::thread(getNoticLoop); // 只能这么写，不这么写会报错
    while(true){
        if(returnKey == order_bulidMay){
            returnKey = order_none;
            std::cout<< "bulid map" << std::endl;
        }
    }
}


int main(int argc, char *argv[])
{
    spdlog::info("bulid map task");
    C_bulidMapApi *g_bulidMap = new C_bulidMapApi();
    eCAL::Initialize(argc, argv, "GNGGA Publisher");
    // manageMpc();
    g_bulidMap->initBulidMap();
    

    while(true){
        usleep(1000000);
    }
}